//
// Created by Wirano on 2020/11/9.
//

#ifndef ANO_ATTCTRL_H
#define ANO_ATTCTRL_H

/* Includes ------------------------------------------------------------------*/
#include "Ano_FcData.h"
#include "Ano_Filter.h"
#include "Ano_Math.h"
#include "Ano_Pid.h"
/* Exported types ------------------------------------------------------------*/

enum
{
	ROLL_END=0,
	ROLL_UP,
	ROLL_ROLLING,
	ROLL_KEEP,
};

typedef struct
{
	uint8_t roll_mode;
	uint8_t rolling_step;
	uint8_t roll_thr_step;
	
	float rol_angle[2];
	
	int16_t ref_height;
	int16_t up_height;
	
	uint16_t keep_cnt;
	int16_t roll_up_speed;
	uint8_t roll_height_ok;
//	int16_t roll_acc_fix;
}_rolling_flag_st;
extern _rolling_flag_st rolling_flag;

typedef struct
{
	float set_yaw_speed;
	
	float exp_angular_velocity[VEC_RPY];

	float fb_angular_velocity[VEC_RPY];
}_att_1l_ct_st;
extern _att_1l_ct_st att_1l_ct;

typedef struct
{
	float yaw_err;
  float exp_rol_adj;
	float exp_pit_adj;
	
	float exp_rol,exp_pit,exp_yaw;
	float fb_rol,fb_pit,fb_yaw;

}_att_2l_ct_st;// 角度环变量结构体
extern _att_2l_ct_st att_2l_ct;

/* Exported constants --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
/* Exported functions ------------------------------------------------------- */
void Att_2level_PID_Init(void);
void Att_1level_PID_Init(void);
void Set_Att_1level_Ki(uint8_t mode);
void Set_Att_2level_Ki(uint8_t mode);

void Att_2level_Ctrl(float dT,int16_t *CH_N);
void Att_1level_Ctrl(float dT);


extern _PID_arg_st arg_2[VEC_RPY] ; 

extern _PID_arg_st arg_1[VEC_RPY] ;

extern _PID_val_st val_2[VEC_RPY];

extern _PID_val_st val_1[VEC_RPY];

#endif //ANO_ATTCTRL_H
